/*************************************************************************
    Projet  -  Sport Collectif pour Robots
                             -------------------
    Auteur  -  Loic Thouvenin
*************************************************************************/

//---------------- Realisation du module <util> (fichier util.cpp) -------

//---------------------------------------------------------------- INCLUDE
//-------------------------------------------------------- Include systeme
using namespace std;

//------------------------------------------------------ Include personnel
#include "util.h"


//----------------------------------------------------------------- PUBLIC

//---------------------------------------------------- Fonctions publiques

long squareDistance(CvPoint *point0, CvPoint *point1)
{
    return (point0->x - point1->x)*(point0->x - point1->x) + (point0->y - point1->y)*(point0->y - point1->y);
}

bool calculerVitesse(const Position & prevPos, const Position & lastPos, Vec2d & vitesse)
{
	bool cr = true;
	long millisElapsed = lastPos.timestamp - prevPos.timestamp;
	if (millisElapsed <= 0)
	{
		cr = false;
	}

	vitesse[0] = ( lastPos.coord[0] - prevPos.coord[0] ) / millisElapsed * 1000;
	vitesse[1] = ( lastPos.coord[1] - prevPos.coord[1] ) / millisElapsed * 1000;

	return cr;
} //----- Fin de <calculerVitesse>

/* ####### Add by Matuchifra ####### */
long computeTimeStamp()
{
	timeval time;
	gettimeofday(&time, NULL);
	return time.tv_sec*1000 + time.tv_usec/1000 + 0.5;
}

double norme(const Vec2d & v)
{
	return sqrt(v[X]*v[X] + v[Y]*v[Y]);
}
